#include "BspM3_Driver.h"
#include "stm32f4xx_hal.h"
#include "cmsis_os2.h"
#include "stm32f4xx.h"
#include "spi.h"
#include "us_delay.h"


#define M3BSP_CS(X) HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, (X) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define M3BSP_DEN(X) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_7, (X) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define M3BSP_NRST(X) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (X) ? GPIO_PIN_SET : GPIO_PIN_RESET)

#define CSDELAY_COUNT   1
static void M3_NFREEZE(uint8_t reset) /*motor halt low active*/
{
	if(reset)
		HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET);
	else
		HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_SET);
}

TypeDefTMC4361_RegStruct TMC4361_M3RegStruct={
    .bsp_write_reg = M3_HardWareSendDat,
    .bsp_read_reg = M3_HardWareReadDat,
    .PROTO_V = 268500/2,//805500, // 转速*895 这个值大概900转
	.bsp_nfreeze = M3_NFREEZE,
};

extern SPI_HandleTypeDef hspi1;


static void M3_IO_Config(void) {
    __HAL_RCC_GPIOA_CLK_ENABLE();
		__HAL_RCC_GPIOE_CLK_ENABLE();
	
    GPIO_InitTypeDef GPIO_InitStruct = {0};
		HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_SET);//PC7为M2急停引脚，低有效
    // 初始化 PC6 PC7 PA8
    GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_11;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
		GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
		GPIO_InitStruct.Pin = GPIO_PIN_4;
		HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

}

uint8_t M3_SPI_Config(void) {
//    Bsp_SPI2_Config();
//    M1_CS_IO_Config();
		MX_SPI1_Init();
    M3_IO_Config();
    M3BSP_DEN(0);
    HAL_Delay(50);
    M3BSP_NRST(0);
    HAL_Delay(50);
    M3BSP_NRST(1);
    HAL_Delay(50);
    return 0; // 假设配置成功返回 0
}

uint8_t M3_HardWareSendDat(uint8_t RegAddr, uint32_t Data) {
//    __disable_irq();
    uint8_t Status = 0;
		uint8_t data_buffer[5];
		RegAddr |= 0x80;
	
/* Split the 32-bit data into 4 bytes */
		data_buffer[0] = RegAddr;
		data_buffer[1] = (Data >> 24) & 0xFF;
		data_buffer[2] = (Data >> 16) & 0xFF;
		data_buffer[3] = (Data >> 8) & 0xFF;
		data_buffer[4] = Data & 0xFF;
	/* Transmit data */

    M3BSP_CS(0);
    delay_us(CSDELAY_COUNT);
		if (HAL_SPI_TransmitReceive(&hspi1, data_buffer, data_buffer, 5, HAL_MAX_DELAY) != HAL_OK)
		{
    /* Transmission error */
    ERROR_HANDLER;
		}
		Status=data_buffer[0];
    M3BSP_CS(1);
    delay_us(CSDELAY_COUNT);
//    __enable_irq();
    return Status;
}

uint8_t M3_HardWareReadDat(uint8_t RegAddr, uint32_t *Data) {
//    __disable_irq();
    uint8_t Status = 0;
		uint8_t data_buffer[5];
		data_buffer[0] = RegAddr;
		data_buffer[1] = 0;
		data_buffer[2] = 0;
		data_buffer[3] = 0;
		data_buffer[4] = 0;
    M3BSP_CS(0);
		delay_us(CSDELAY_COUNT);
		if (HAL_SPI_Transmit(&hspi1, data_buffer, 5, HAL_MAX_DELAY) != HAL_OK)
		{
    /* Transmission error */
    ERROR_HANDLER;
		}
    M3BSP_CS(1);
    delay_us(CSDELAY_COUNT);
//		uint8_t data_tx_buffer[5] = {0,0, 0, 0, 0};
    uint8_t data_rx_buffer[5];
    M3BSP_CS(0); // 拉低片选信号
    delay_us(CSDELAY_COUNT); // 确保片选信号稳定
	HAL_StatusTypeDef err = HAL_SPI_Receive(&hspi1, data_rx_buffer, 5, HAL_MAX_DELAY);
    if (err != HAL_OK) {
        // 传输错误
        ERROR_HANDLER;
    }

    M3BSP_CS(1); // 拉高片选信号
    delay_us(CSDELAY_COUNT); // 确保片选信号稳定
		Status=data_rx_buffer[0];
		*Data = ((uint32_t)data_rx_buffer[1] << 24) | ((uint32_t)data_rx_buffer[2] << 16) | ((uint32_t)data_rx_buffer[3] << 8) | data_rx_buffer[4];

//    __enable_irq();
    return Status;
}

void M3_Init(void)
{
	M3_SPI_Config();
	
	M3_HardWareSendDat(0x4F,0x52535400); //reset
	osDelay(100);
	//general setup:
	//----------------------
	M3_HardWareSendDat(0X03,0X540022);      // Inputfilter: START und Encoderpins   输入滤波器配置
	M3_HardWareSendDat(0X04,0X8440000d);   //Setup of SPI Output Timing  SPI输出配置寄存器
	M3_HardWareSendDat(0X00,0x6026);       // direct-a, direct-bow  通用配置寄存器
	M3_HardWareSendDat(0X10,0X60004);       // step length   步进方向输出配置寄存器
	//configuration of TMC2130:   2130 配置寄存器
	//----------------------
	
	M3_HardWareSendDat(0X6D,0X81);      // GCONF
	M3_HardWareSendDat(0X6C,0X07);       // diag1=index, pushpull, direct_mode = on --> SPI mode
	M3_HardWareSendDat(0X6D,0X80);      // GCONF
	M3_HardWareSendDat(0X6C,0X12200);       // diag1=index, pushpull, direct_mode = on --> SPI mode
	M3_HardWareSendDat(0X6D,0X90);      // IHOLD_IRUN
	
#ifdef MOTOR_OPENLOOP
	M3_HardWareSendDat(0X6C,0X00070603);        // ihold=0, iholddelay=7, irun=2
#else
	M3_HardWareSendDat(0X6C,0X00070A04);        // ihold=0, iholddelay=7, irun=2
#endif
	M3_HardWareSendDat(0X6D,0X91);     // TPOWERDOWN
	M3_HardWareSendDat(0X6C,0Xa);           // TZEROWAIT=10
	M3_HardWareSendDat(0X6D,0X94);      // TCOOLTHRS
	M3_HardWareSendDat(0X6C,0Xc8);          // =200
	M3_HardWareSendDat(0X6D,0X95);      // THIGH
	M3_HardWareSendDat(0X6C,0X32);          // =50
	M3_HardWareSendDat(0X6D,0Xee);      // DCCTRL
	M3_HardWareSendDat(0X6C,0X70025);       // DCCTRLSG=5, DC_TIME=37
	M3_HardWareSendDat(0X6D,0Xf0);      // PWMCONF
	M3_HardWareSendDat(0X6C,0X404ff);       // 
	M3_HardWareSendDat(0X6D,0Xec);      // CHOPCONF
	M3_HardWareSendDat(0X6C,0X10223);       //
	M3_HardWareSendDat(0X6D,0Xed);      // COOLCONF (off)
	M3_HardWareSendDat(0X6C,0X100a220);     //
	M3_HardWareSendDat(0X6D,0X8B);      // GLOBAL_SCALER (off)
	M3_HardWareSendDat(0X6C,0X0);   //          global scaler = 0

	//清零速度位置目标位置
	M3_HardWareSendDat(0X24,0X0);  // v=0      VMAX  
	M3_HardWareSendDat(0X21,0X0);  // xact=0   XACTUAL
	M3_HardWareSendDat(0X37,0X0);  // xtarg=0  XTARGET
	M3_HardWareSendDat(0X50,0);    // enc_pos

#ifdef MOTOR_OPENLOOP
#else
	//cl basic parameter  闭环基础参数配置
	//----------------------
	M3_HardWareSendDat(0X54,4000);        // encoder resolution = 40k/rev ADAPT THIS PARAMETER to SETUP   编码器分辨率配置寄存器
	M3_HardWareSendDat(0X1C,0Xff00ff);      // CL_BETA=CL_GAMMA=255  闭环调节
	M3_HardWareSendDat(0X5C,0X10000);      // CL_DELTA_P = 1.0    PID_D


	//cl calibration:  闭环校准
	//----------------------
	M3_HardWareSendDat(0X20,0X6);       // ramp mode: hold+pos mode     运行模式
	M3_HardWareSendDat(0X24,0X100000);      // slow vel cl calibration: vmax
	
	M3_HardWareSendDat(0X07,0X400000);      // regulation_modus: turn on cl  编码器信号配置寄存器
//	M1_HardWareSendDat(0X37,0X80);      // move to FS position      目标位置寄存器
	M3_HardWareSendDat(0X07,0X1400000);     // cl_calibration_en: turn on cl calibration    编码器信号配置寄存器
	osDelay(100);     //  延时10MS
	M3_HardWareSendDat(0X07,0X400000);      // cl_calibration_en: turn off cl calibration   编码器配置寄存器
	M3_HardWareSendDat(0X24,0X0);       // v=0    VMAX
#endif
	//S-shaped Ramps setup:  S 加减速参数配置
	//----------------------
	M3_HardWareSendDat(0X20,0X6);       // ramp mode: hold+pos mode     运行模式
	M3_HardWareSendDat(0X24,0X05350000);       // vmax=400kpps    VMAX
	M3_HardWareSendDat(0X20,0X06);       // s-ramp + pos mode     运行模式

	//ramp generator    S 曲线斜坡发生器参数
	M3_HardWareSendDat(0X28,30000);     // amax  每平方秒的脉冲
	M3_HardWareSendDat(0X29,30000);     // dmax  每平方秒的脉冲
	M3_HardWareSendDat(0X2D,0X28);     // bow1  每立方秒的脉冲
	M3_HardWareSendDat(0X2E,0X28);     // bow2  每立方秒的脉冲
	M3_HardWareSendDat(0X2F,0X28);     // bow3  每立方秒的脉冲
	M3_HardWareSendDat(0X30,0X28);     // bow4  每立方秒的脉冲

#ifdef MOTOR_OPENLOOP
#else
	//cl operation setup:     闭环设置
	//----------------------
	// back-emf Compensation   反电动势补偿设置
	M3_HardWareSendDat(0X60,250000);       // emf_vmin=
	M3_HardWareSendDat(0X61,450000);       // emf_vadd= -> emf_vmax= 
	M3_HardWareSendDat(0X62,0Xffffff);      // emc_vel0_timer (enc_vel_zero, clock cycles)   编码器延迟时间
	M3_HardWareSendDat(0X63,0X2000864);     // enc vel filter settings (v_enc_mean)   编码器速度配置寄存器
									
	// position Deviation
	M3_HardWareSendDat(0X5F,5);      // cl_tolerance=20     PID 参数寄存器
	M3_HardWareSendDat(0X5C,0X10000);       // cl_p=1.0     PID_D
									
	// catch up velocity   追赶速度参数设置
	M3_HardWareSendDat(0X5A,100);      // cl_vlimit_p=200     PID_P
	M3_HardWareSendDat(0X5B,50);      // cl_vlimit_i=50       PID_I
	M3_HardWareSendDat(0X5D,1000);     // cl_vlimit_diclip=1000   PID_D
	M3_HardWareSendDat(0X5E,200000);       // cl_vlimit=100kpps         PID限幅寄存器

	// closed loop scaling   闭环电流调节参数设置
	M3_HardWareSendDat(0X06,0x32ff64);      // cl scaling values   电流调节寄存器
	M3_HardWareSendDat(0X18,1000);     // cl_upscale=    电流延迟周期寄存器
	M3_HardWareSendDat(0X19,100000);       // cl_dnscale=  电流递减延迟周期寄存器
	M3_HardWareSendDat(0X07,0X0A400000);        // cl encoder signal configuration with gamma correction and vlimit (catch up velocity)     编码器信号配置寄存器

	M3_HardWareSendDat(0X05,0X80);      // cl scaling on  电流调节配置寄存器
	M3_HardWareSendDat(0X62,50000);      // 设置编码器速度更新时钟周期数
	M3_HardWareSendDat(0X53,500);      // ENC_POS_DEV_TOL 设置编码器ENC_POS_DEV 的最大容差值
	M3_HardWareSendDat(0X52,256);				//这里不写的话 TARGET_REACHED不会触发
//	M3_HardWareSendDat(0X0E,000);
#endif
	osDelay(10);
}
